package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.subsystems.Arm;
import org.firstinspires.ftc.teamcode.subsystems.ArmSystem;
import org.firstinspires.ftc.teamcode.subsystems.CarouselSystem;
import org.firstinspires.ftc.teamcode.subsystems.DeviceBase;
import org.firstinspires.ftc.teamcode.subsystems.HeadSystem;
import org.firstinspires.ftc.teamcode.subsystems.IMU;
import org.firstinspires.ftc.teamcode.subsystems.IntakeSystem;
import org.firstinspires.ftc.teamcode.subsystems.LaunchSystem;
import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveWrapper;
import org.firstinspires.ftc.teamcode.subsystems.Navigation;
import org.firstinspires.ftc.teamcode.subsystems.Ranger;
import org.firstinspires.ftc.teamcode.subsystems.TensorFlowObjectDetection;

import java.util.ArrayList;

public class RobotAuto {

    public MecanumDriveWrapper drive;
    public HeadSystem head;
    public CarouselSystem carousel;
    public IntakeSystem intake;
    public ArmSystem arm;
    public TensorFlowObjectDetection detector;
    public IMU imu;

    public int detectResult = 0;

    ArrayList<DeviceBase> devices = new ArrayList<>();

    public RobotAuto() {

        drive = new MecanumDriveWrapper();
        head = new HeadSystem();
        carousel = new CarouselSystem();
        intake = new IntakeSystem();
        arm = new ArmSystem();
        detector = new TensorFlowObjectDetection();
        imu = new IMU();
        devices.add(drive);
        devices.add(head);
        devices.add(carousel);
        devices.add(intake);
        devices.add(detector);
        devices.add(arm);

    }

    public void init(HardwareMap hardwareMap) {
        for (DeviceBase d : devices) {
            d.init(hardwareMap);
        }
    }
    public void loop(double time){
        for (DeviceBase d : devices) {
            d.loop(time);
        }
    }
    public void stop(){
        for (DeviceBase d : devices) {
            d.stop();
        }
    }
    public void autoLoop(double time) {
        for (DeviceBase d : devices) {
            d.autoLoop(time);
        }
    }
}
